#include "rclcpp/rclcpp.hpp"
#include <shared_parameters/shared_params_interface.hpp>

class NodeManager {
public:
    static NodeManager& getInstance() {
        static NodeManager instance;
        return instance;
    }

    void initialize(int argc, char** argv) {
        if (!node_) {
            rclcpp::init(argc, argv);
            node_ = std::make_shared<rclcpp::Node>("navigator");
            init_parameters();
        }
    }

    rclcpp::Node::SharedPtr getNode() { return node_; }
    const rclcpp::Time getTime() { return node_->now(); }
    const char* getName() { return node_->get_name(); }
private:
    NodeManager() = default;
    ~NodeManager() {
        if (rclcpp::ok()) {
            rclcpp::shutdown();
        }
    }
    
    NodeManager(const NodeManager&) = delete;
    NodeManager& operator=(const NodeManager&) = delete;
    
    bool init_parameters() {
        if (!SharedParamsInterface::initialize()) {
            RCLCPP_ERROR(rclcpp::get_logger("navigator"), "Failed to initialize shared memory interface");
            return false;
        }

        try { 
            auto params = SharedParamsInterface::getNodeParams("shared_parameters");
            if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
              RCLCPP_ERROR(rclcpp::get_logger("navigator"), "Error get shared parameters");
              return false;
            } 

            params = SharedParamsInterface::getNodeParams("agv_navigate_node");
            for (const auto& [name, value] : params) {
              RCLCPP_INFO_STREAM(rclcpp::get_logger("navigator"), "get param:" << name << ", value:" << value.toString());
              switch(value.type) {
                case SharedParamsInterface::ParamValue::Type::INT:
                    node_->declare_parameter(name, value.getValue<int>());
                    break;
                case SharedParamsInterface::ParamValue::Type::DOUBLE:
                    node_->declare_parameter(name, value.getValue<double>());
                    break;
                case SharedParamsInterface::ParamValue::Type::STRING:
                    node_->declare_parameter(name, value.getValue<std::string>());
                    break;
                case SharedParamsInterface::ParamValue::Type::BOOL:
                    node_->declare_parameter(name, value.getValue<bool>());
                    break;
                default:
                    RCLCPP_ERROR_STREAM(rclcpp::get_logger("navigator"), "Unsupported parameter type for " << name);
                    break;
              }
            }
        } catch (const std::exception& e) {
            RCLCPP_ERROR_STREAM(rclcpp::get_logger("navigator"), "Error reading parameters: " << e.what());
            return false;
        }
        return true;
    }

    rclcpp::Node::SharedPtr node_; 
};

inline rclcpp::Node::SharedPtr ROSNode() {
    return NodeManager::getInstance().getNode();
}

inline const rclcpp::Time ROSTime() {
    return NodeManager::getInstance().getTime();
}

inline const char* NodeName() {
    return NodeManager::getInstance().getName();
}